#pragma once

#include <math.h>

namespace GenericMath
{
	extern double eps;
	extern double pi;
	extern double halfPi;
	extern double twoPi;
	extern double piOver2;
	extern double piOver4;
	extern double piOver3;
	extern double piOver6;
	extern double sqrt3;
	extern double sqrt2;
	extern double oneOverSqrt2;
	extern double oneOverSqrt3;

	double PoorMansGaussian();
	double PoorMansGaussian(double lfRandIn);

	double RandRange(double lfMin, double lfMax);

	
};


template <int TEMPL_VEC_DIM, class ARG_TYPE>
class mdsVector
{
	ARG_TYPE m_arrVals[TEMPL_VEC_DIM];
public:
	mdsVector(void)
	{
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			m_arrVals[i] = (ARG_TYPE)0.0;
		}
	}

	mdsVector(bool bInit)
	{
		if(bInit)
		{
			for(int i = 0; i < TEMPL_VEC_DIM; ++i)
			{
				m_arrVals[i] = (ARG_TYPE)0.0;
			}
		}
	}

	mdsVector(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE>& vecIn)
	{
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			m_arrVals[i] = vecIn.m_arrVals[i];
		}
	}

	~mdsVector(void)
	{
		
	}

	inline ARG_TYPE operator[](int idx) const
	{
		return m_arrVals[idx];
	}

	inline ARG_TYPE & operator[](int idx)
	{
		return m_arrVals[idx];
	}

	inline const ARG_TYPE x() const { return m_arrVals[0]; }
	inline const ARG_TYPE y() const { return m_arrVals[1]; }
	inline const ARG_TYPE z() const { return m_arrVals[2]; }

	inline ARG_TYPE & x() { return m_arrVals[0]; }
	inline ARG_TYPE & y() { return m_arrVals[1]; }
	inline ARG_TYPE & z() { return m_arrVals[2]; }
	// virtual int GetDim() const { return TEMPL_VEC_DIM; }

	ARG_TYPE Dot(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & dotWith) const
	{
		ARG_TYPE fResult = (ARG_TYPE)0.0;
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			fResult += m_arrVals[i]*dotWith.m_arrVals[i];
		}
		return fResult;
	}

	ARG_TYPE GetMag() const
	{
		ARG_TYPE fResult = this->Dot(this);
		return (ARG_TYPE)(sqrt(fResult));
	}

	mdsVector<TEMPL_VEC_DIM, ARG_TYPE> GetNorm() const
	{
		mdsVector<TEMPL_VEC_DIM, ARG_TYPE> vecResult(*this);
		ARG_TYPE fMagSqrd = this->Dot(*this);
		if(fMagSqrd <= 0.0)
		{
			return vecResult;
		}
		double fDivideBy = 1.0/sqrt((double)fMagSqrd);
		vecResult *= ((ARG_TYPE)fDivideBy);
		return vecResult;
	}

	bool Normalize()
	{
		ARG_TYPE fMagSqrd = this->Dot(*this);
		if(fMagSqrd <= 0.0)
		{
			return false;
		}
		double fDivideBy = 1.0/sqrt((double)fMagSqrd);
		*this *= (ARG_TYPE)fDivideBy;
		return true;
	}

	mdsVector<TEMPL_VEC_DIM, ARG_TYPE> operator+(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & addMe) const
	{
		mdsVector<TEMPL_VEC_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			vecResult[i] = addMe.m_arrVals[i] + m_arrVals[i];
		}
		return vecResult;
	}

	const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> operator+=(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & addMe) 
	{
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			m_arrVals[i] += addMe.m_arrVals[i];
		}
		return *this;
	}

	mdsVector<TEMPL_VEC_DIM, ARG_TYPE> operator-(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & addMe) const
	{
		mdsVector<TEMPL_VEC_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			vecResult[i] = -addMe.m_arrVals[i] + m_arrVals[i];
		}
		return vecResult;
	}

	const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> operator-=(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & addMe) 
	{
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			m_arrVals[i] -= addMe.m_arrVals[i];
		}
		return *this;
	}

	mdsVector<TEMPL_VEC_DIM,ARG_TYPE> operator*(ARG_TYPE lfMul) const
	{
		mdsVector<TEMPL_VEC_DIM,ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			vecResult.m_arrVals[i] = lfMul*m_arrVals[i];
		}
		return vecResult;
	}

	ARG_TYPE operator*(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE>& vecIn) const
	{
		ARG_TYPE result = (ARG_TYPE)0.0;
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			result += m_arrVals[i]*vecIn.m_arrVals[i];
		}
		return result;
	}

	const mdsVector<TEMPL_VEC_DIM,ARG_TYPE> & operator*=(ARG_TYPE lfMul)
	{
		
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			m_arrVals[i] = lfMul*m_arrVals[i];
		}
		return *this;
	}

	ARG_TYPE * GetArr() // MDS -- does this work?
	{
		return m_arrVals;
	}

	const ARG_TYPE * GetArr() const // MDS -- does this work?
	{
		return m_arrVals;
	}

	const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & operator=(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & src)
	{
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			m_arrVals[i] = src.m_arrVals[i];
		}
		return *this;
	}

	//vectorBase ProjectUp() const
	//{
	//	mdsVector<TEMPL_VEC_DIM+1, ARG_TYPE> vecResult;
	//	for(int i = 0; i < TEMPL_VEC_DIM; ++i)
	//	{
	//		vecResult[i] = m_arrVals[i];
	//	}
	//	// memcpy(vecResult.m_arrVals, m_arrVals, TEMPL_VEC_DIM*sizeof(ARG_TYPE));
	//	vecResult[TEMPL_VEC_DIM] = (ARG_TYPE)1.0;
	//	return vecResult;
	//}

	//vectorBase ProjectDown() const
	//{
	//	mdsVector<TEMPL_VEC_DIM-1, ARG_TYPE> vecResult;
	//	double lfWInverse = (double)m_arrVals[TEMPL_VEC_DIM-1];
	//	if(lfWInverse == 0.0 || lfWInverse == -0.0)
	//	{
	//		vecError errResult;
	//		return errResult;
	//	}
	//	lfWInverse = 1.0/lfWInverse;
	//	for(int i = 0; i < TEMPL_VEC_DIM-1; ++i)
	//	{
	//		vecResult[i] = (ARG_TYPE)(m_arrVals[i]*lfWInverse);
	//	}
	//	return vecResult;
	//}

	mdsVector<TEMPL_VEC_DIM, ARG_TYPE> Cross(const mdsVector<TEMPL_VEC_DIM, ARG_TYPE> & vecIn) const // note:  is not an actual cross product for vectors of lengths other then 3
	{
		mdsVector<TEMPL_VEC_DIM, ARG_TYPE> vecResult;
		for(int i = 0; i < TEMPL_VEC_DIM; ++i)
		{
			int iPlusOne = (i+1)%TEMPL_VEC_DIM;
			int iPlusTwo = (i+2)%TEMPL_VEC_DIM;
			vecResult.m_arrVals[i] = 
				m_arrVals[iPlusOne] * vecIn.m_arrVals[iPlusTwo]
				- vecIn.m_arrVals[iPlusOne] * m_arrVals[iPlusTwo];
		}
		return vecResult;
	}
};

namespace OctUtils
{

	inline int Vec3To3dQuadrant(const mdsVector<3, double> & vecIn)
	{
		return (vecIn[0] > 0.0f) + 2*(vecIn[1] > 0.0f) +4*(vecIn[2] > 0.0f);
	}

	inline void QuadrantToMdsVector(int nQuad, mdsVector<3, double> & vecOut)
	{
		for(int i = 0; i < 3; ++i)
		{
			if(!(nQuad&1))
			{
				vecOut[i] *= -1.0;
			}
			nQuad = nQuad>>1;
		}
	}

	extern int QuadTraverseOrder[8];

	extern int OctCornerGreyCode[8];

};

template <class ARG_TYPE>
struct mdsRay
{
	mdsVector<3, ARG_TYPE> m_p3Dir;
	mdsVector<3, ARG_TYPE> m_p3Pos;

	mdsRay() {m_p3Dir[2] = 1; }

	mdsRay(const mdsRay<ARG_TYPE> & src)
	{
		m_p3Dir = src.m_p3Dir;
		m_p3Pos = src.m_p3Pos;
	}

	const mdsRay<ARG_TYPE> & operator=( const mdsRay<ARG_TYPE> & src)
	{
		m_p3Dir = src.m_p3Dir;
		m_p3Pos = src.m_p3Pos;
		return *this;
	}

	ARG_TYPE MinDistToPointSqrd(const mdsVector<3, ARG_TYPE> & vecSphereCenter) const
	{
		mdsVector<3, ARG_TYPE> vecToCenter = vecSphereCenter-m_p3Pos;
		ARG_TYPE fDistToPerpPlane = m_p3Dir.Dot(vecToCenter);
		ARG_TYPE fResult = fDistToPerpPlane*fDistToPerpPlane/(m_p3Dir.Dot(m_p3Dir));
		return vecToCenter.Dot(vecToCenter)-fResult;
	}

	bool RayHitPlane(int nAxis, ARG_TYPE fPlaneVal, mdsVector<3, ARG_TYPE> & vecIsect) const
	{
		if((fPlaneVal-m_p3Pos[nAxis])*m_p3Dir[nAxis] > 0)
		{
			vecIsect = m_p3Pos;
			ARG_TYPE fXlateBy = (fPlaneVal-m_p3Pos[nAxis])/m_p3Dir[nAxis];
			vecIsect[0] += fXlateBy*m_p3Dir[0];
			vecIsect[1] += fXlateBy*m_p3Dir[1];
			vecIsect[2] += fXlateBy*m_p3Dir[2];
			return true;
		}
		return false;
	}
};



